#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     leftFront,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     rightFront,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     leftBack,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     rightBack,     tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void forward(int p, int d){
	motor[leftFront] = p;
	motor[rightFront] = p;
	motor[leftBack] = p;
	motor[rightBack] = p;
  wait10Msec(d);
  motor[leftFront] = 0;
	motor[rightFront] = 0;
	motor[leftBack] = 0;
	motor[rightBack] = 0;
}
void right(int p, int d){
	motor[leftFront] = -p;
	motor[rightFront] = p;
	motor[leftBack] = p;
	motor[rightBack] = -p;
  wait10Msec(d);
  motor[leftFront] = 0;
	motor[rightFront] = 0;
	motor[leftBack] = 0;
	motor[rightBack] = 0;

}
void left(int p, int d){
	motor[leftFront] = p;
	motor[rightFront] = -p;
	motor[leftBack] = -p;
	motor[rightBack] = p;
  wait10Msec(d);
  motor[leftFront] = 0;
	motor[rightFront] = 0;
	motor[leftBack] = 0;
	motor[rightBack] = 0;

}
void forwardRight(int p, int d){
	motor[rightFront] = p;
	motor[leftBack] = p;
  wait10Msec(d);
	motor[rightFront] = 0;
	motor[leftBack] = 0;

}
void forwardLeft(int p, int d){
	motor[leftFront] = p;
	motor[rightBack] = p;
  wait10Msec(d);
	motor[leftFront] = 0;
	motor[rightBack] = 0;

}
void turn(int p, int d){
	motor[leftFront] = p;
	motor[rightFront] = -p;
	motor[leftBack] = p;
	motor[rightBack] = -p;
  wait10Msec(d);
  motor[leftFront] = 0;
	motor[rightFront] = 0;
	motor[leftBack] = 0;
	motor[rightBack] = 0;
}

task main(){
	//All time in this program is done with 10Msec, 100 = 1 second
	//
	//forwardLeft and forwardRight both can take - to reverse int backwardsLeft and backwardsRight
  //for ward takes a - to go backwards

	//forward
	forward(75, 100);
	wait10Msec(200);
	//backward
	forward(-75, 100);
	wait10Msec(200);
	//pan right
	right(25, 100);
	wait10Msec(200);
	//pan left
	left(25, 100);
	wait10Msec(200);
	//diagonal forward right
	forwardRight(75, 100);
	wait10Msec(200);
	//diagonal bacwards left
	forwardRight(-75, 100);
	wait10Msec(200);
	//diagonal forward left
	forwardLeft(75, 100);
	wait10Msec(200);
	//diagonal backwards right
	forwardLeft(-75, 100);
	wait10Msec(200);
	//turn to the right tank style
	turn(75, 100);
	wait10Msec(200);
	//turn to the left tank style
	turn(-75, 100);
	wait10Msec(200);


}
